#include <ros/ros.h>

#include "cg_interfaces/ChassisControl.h"
#include "cg_interfaces/ChassisSensor.h"
#include "cg_interfaces/BridgeControl.h"
#include "cg_interfaces/BridgeSensor.h"
#include "cg_interfaces/ChassisStruct.h"

ros::Subscriber control_sub;
ros::Publisher sensor_pub;
ros::Publisher bridge_control_pub;
ros::Subscriber bridge_sensor_sub;

cg_interfaces::BridgeSensor bridge_sensor;

void OnChassisControl(const cg_interfaces::ChassisControl::ConstPtr &msg)
{
    if (msg->count != 1)
    {
        ROS_WARN("chassis: bridge count is not right");
        return;
    }
    bridge_control_pub.publish(msg->bridges[0]);
}

void OnBridgeSensor(const cg_interfaces::BridgeSensor::ConstPtr &msg)
{
    bridge_sensor = *msg;
}

void OnTimer(const ros::TimerEvent &event)
{
    cg_interfaces::ChassisSensor sensor_msg;
    sensor_msg.bridge_count = 1;
    sensor_msg.bridges[0] = bridge_sensor;
    sensor_pub.publish(sensor_msg);
}

main(int argc, char **argv)
{
    ros::init(argc, argv, "mono_chassis");
    ROS_INFO("Starting mono_chassis node");

    ros::NodeHandle nh;
    ros::NodeHandle localHandle("/bridge1");

    control_sub = nh.subscribe<cg_interfaces::ChassisControl>("chassis_control", 1, OnChassisControl);
    sensor_pub = nh.advertise<cg_interfaces::ChassisSensor>("chassis_sensor", 1);
    bridge_control_pub = localHandle.advertise<cg_interfaces::BridgeControl>("control", 1);
    bridge_sensor_sub = localHandle.subscribe<cg_interfaces::BridgeSensor>("sensor", 1, OnBridgeSensor);
    auto timer = nh.createTimer(ros::Duration(1.0 / 50), OnTimer);

    ros::spin();
    return 0;
}